#include "tcv_dmhandle.h"
#include "tcv_config.h"
#include <opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/video.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/flann.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/ml.hpp>
#include <QDebug>
#include <QWidget>

TCV_DMHandle::TCV_DMHandle():
    m_imageWidth(_Camera_Frame_Width),
    m_imageHeight(_Camera_Frame_Height)
//    ,mc_matQueueSizeLimit(10)
{
    m_cameraIdPl=0;
    m_cameraIdVr=0;

//    m_VideoCapture_Pl=NULL;
//    m_VideoCapture_Vr=NULL;
}

bool TCV_DMHandle::SearchCameraID(std::vector<unsigned int> &o_cameraID)
{
    cv::VideoCapture t_checkVideoID;
    for(int i=0;i<10;i++)
    {
        if(t_checkVideoID.open(i)==true)
        {
            o_cameraID.push_back(i);
            t_checkVideoID.release();
        }
    }
    if(o_cameraID.empty())
        return false;
    else
        return true;
}

bool TCV_DMHandle::OpenTwoCamera(unsigned int i_cameraId1, unsigned int i_cameraId2, int &o_errorNum)
{
//    m_VideoCapture_Pl.open(i_cameraId1);
    return false;
}

bool TCV_DMHandle::OpenOneCamera(unsigned int i_cameraId, int &o_errorNum)
{
    bool t_judge = m_VideoCapture_Pl.open(i_cameraId);
    if(t_judge){
        o_errorNum=0;
        m_cameraIdPl = 0;
        return true;
    }
    else{
        o_errorNum=-1;
        return false;
    }
    m_VideoCapture_Pl.set(cv::CAP_PROP_FRAME_WIDTH,m_imageWidth);
    m_VideoCapture_Pl.set(cv::CAP_PROP_FRAME_HEIGHT,m_imageHeight);
}

bool TCV_DMHandle::IsCameraOpen(unsigned int i_whichOneInt)
{
    if(i_whichOneInt == 1){
        if(m_VideoCapture_Pl.isOpened()){
            return true;
        }
    }
    else if(i_whichOneInt == 2){
        if(m_VideoCapture_Vr.isOpened()){
            return true;
        }
    }
    return false;
}

bool TCV_DMHandle::CloseCarmera()
{
    if(m_VideoCapture_Pl.isOpened())
        m_VideoCapture_Pl.release();
    if(m_VideoCapture_Vr.isOpened())
        m_VideoCapture_Vr.release();
    return true;
}


bool TCV_DMHandle::CatchPicture(unsigned char i_whichOneU8)
{
//    cv::Mat t_mat;
    if((i_whichOneU8&0x0F) == 0x01){
        if(!m_VideoCapture_Pl.isOpened())
            return false;
        m_VideoCapture_Pl >> m_Mat_PicturePl;
        if(m_Mat_PicturePl.empty())
            return false;
    }
    else if((i_whichOneU8&0xF0) == 0x10){
        if(!m_VideoCapture_Vr.isOpened())
            return false;
        m_VideoCapture_Vr >> m_Mat_PictureVr;
        if(m_Mat_PictureVr.empty())
            return false;
    }
    return true;
}

bool TCV_DMHandle::CatchPicture(unsigned char i_whichOneU8, int &o_errorNum)
{
    cv::Mat t_mat;
    if(i_whichOneU8&0x0F == 0x01){
        if(!m_VideoCapture_Pl.isOpened()){
            o_errorNum = 11;
            return false;
        }
        m_VideoCapture_Pl >> m_Mat_PicturePl;
        if(m_Mat_PicturePl.empty()){
            o_errorNum = 12;
            return false;
        }
    }
    else if(i_whichOneU8&0xF0 == 0x10){
        if(!m_VideoCapture_Vr.isOpened()){
            o_errorNum = 21;
            return false;
        }
        m_VideoCapture_Vr >> m_Mat_PictureVr;
        if(m_Mat_PictureVr.empty()){
            o_errorNum = 22;
            return false;
        }
    }
    o_errorNum = 01;
    return true;
}

bool TCV_DMHandle::CopyPicture(unsigned int i_whichOneInt, cv::Mat &o_pictureMat)
{
    if(i_whichOneInt == 1){
        if(m_Mat_PicturePl.empty()){
            return false;
        }
        o_pictureMat = m_Mat_PicturePl;
    }
    else if(i_whichOneInt == 2){
        if(m_Mat_PictureVr.empty()){
            return false;
        }
        o_pictureMat = m_Mat_PictureVr;
    }
    return true;
}

QImage MatToQImage(const cv::Mat& mat)
{
    switch (mat.type())
    {
        // 8-bit, 4 channel
        case CV_8UC4:
            {
                QImage image(mat.data,
                             mat.cols, mat.rows,
                             static_cast<int>(mat.step),
                             QImage::Format_ARGB32);

                return image;
            }

         // 8-bit, 3 channel
         case CV_8UC3:
            {
                QImage image(mat.data,
                             mat.cols, mat.rows,
                             static_cast<int>(mat.step),
                             QImage::Format_RGB888);

                return image.rgbSwapped();
            }

         // 8-bit, 1 channel
        case CV_8UC1:
            {
                #if QT_VERSION >= QT_VERSION_CHECK(5, 5, 0)
                QImage image(mat.data,
                             mat.cols, mat.rows,
                             static_cast<int>(mat.step),
                             QImage::Format_Grayscale8);
                #else
                static QVector<QRgb>  sColorTable;

                // only create our color table the first time
                if (sColorTable.isEmpty())
                {
                    sColorTable.resize( 256 );

                    for ( int i = 0; i < 256; ++i )
                    {
                        sColorTable[i] = qRgb( i, i, i );
                    }
                }

                QImage image(mat.data,
                             mat.cols, mat.rows,
                             static_cast<int>(mat.step),
                             QImage::Format_Indexed8 );

                image.setColorTable(sColorTable);
                #endif

                return image;
            }

        // wrong
        default:
            qDebug() << "ERROR: Mat could not be converted to QImage.";
            break;
    }
    return QImage();
}

bool TCV_DMHandle::CopyPicture(unsigned int i_whichOneInt, QImage &o_pictureQImage)
{
    cv::Mat t_Mat;

    if(i_whichOneInt == 1){
        if(m_Mat_PicturePl.empty()){
            return false;
        }
        t_Mat = m_Mat_PicturePl;
    }
    else if(i_whichOneInt == 2){
        if(m_Mat_PictureVr.empty()){
            return false;
        }
        t_Mat = m_Mat_PictureVr;
    }

    o_pictureQImage = MatToQImage(t_Mat);
    return true;
}

bool TCV_DMHandle::ClonePictureToInputQueue(unsigned char i_whichOneU8)
{
    bool t_returnValue=false;
    //1- if cannot get the mux, error and return
    std::chrono::milliseconds t_timermutTimeout(_TimedmutTimeout);
    if(m_TimedMut_MatQueuePl.try_lock_for(t_timermutTimeout)==false)
        return false;
    //2-if no distance to clone, error and return

    //3.1- clone left/prime
    //if the m_Mat_PicturePl(from) is empty, or m_Queue_MatPl(to) is full, error and return
    if((i_whichOneU8&0x0F)==0x01)
    {
        if(m_Mat_PicturePl.empty()==false && m_Queue_MatPl.size()<mc_matQueueSizeLimit){
            m_Queue_MatPl.push(m_Mat_PicturePl.clone());
            t_returnValue=true;
        }
    }
    //3.2- clone right/vice
    if((i_whichOneU8&0xF0)==0x10)
    {
        if(m_Mat_PictureVr.empty()==false && m_Queue_MatVr.size()<mc_matQueueSizeLimit){
            m_Queue_MatVr.push(m_Mat_PictureVr.clone());
            t_returnValue=true;
        }
        else
            t_returnValue=false;
    }
    m_TimedMut_MatQueuePl.unlock();
    return t_returnValue;
}

bool TCV_DMHandle::PopPictureFromInputQueue(unsigned int i_whichOneInt, cv::Mat &o_matPicture)
{
    //1- if cannot get the mux, error and return
    std::chrono::milliseconds t_timermutTimeout(_TimedmutTimeout);
    if(m_TimedMut_MatQueuePl.try_lock_for(t_timermutTimeout)==false)
        return false;

    //2- get the queue
    std::queue<cv::Mat> *t_queueMat;
    if(i_whichOneInt==1)
        t_queueMat = &m_Queue_MatPl;
    else if(i_whichOneInt==2)
        t_queueMat = &m_Queue_MatVr;
    else
        return false;

    //3- queue empty, error
    if(t_queueMat->size()==0){
        m_TimedMut_MatQueuePl.unlock();
        return false;
    }

    //4-get the picture
    o_matPicture = t_queueMat->front();
    t_queueMat->pop();

    m_TimedMut_MatQueuePl.unlock();
    return true;
}

/* 识别完图片，把图片压进 m_Queue_MatDectOutput 这个队列里面
 * push the dect finish picture to queue , after dect finish
 *
*/
bool TCV_DMHandle::CloneDectResToOutputQueue(cv::Mat i_matPicture)
{
    //1- if queue is empty, must to mutux
    //if cannot get the mux, error and return
//    bool t_isMut = false;
//    if(m_Queue_MatDectOutput.empty()){
//        t_isMut = true;
        std::chrono::milliseconds t_timermutTimeout(_TimedmutTimeout);
        if(m_TimedMut_MatQueuePl.try_lock_for(t_timermutTimeout)==false)
            return false;
//    }
    //2-whether the input mat picture is empty?
    if(i_matPicture.empty()==false){
//        if(m_Queue_MatDectOutput.size()>=mc_matQueueSizeLimit)
//            m_Queue_MatDectOutput.pop();
        m_Queue_MatDectOutput.push(i_matPicture.clone());
    }
    else
        return false;
    //3-unlock
//    if(t_isMut == true)
        m_TimedMut_MatQueuePl.unlock();
    return true;
}

/* 获得识别完成后的图片，从 m_Queue_MatDectOutput 这个队列里面取出来
 * get the dect finish picture
 *
*/
bool TCV_DMHandle::PopDectResFromOutputQueue(cv::Mat &o_matPicture)
{
    bool t_returnValue=false;
    //1- if queue size is 1, must to mutux
    //1- if cannot get the mux, error and return
//    bool t_isMut = false;
//    if(m_Queue_MatDectOutput.size()<=1){
//        t_isMut = true;
        std::chrono::milliseconds t_timermutTimeout(_TimedmutTimeout);
        if(m_TimedMut_MatQueuePl.try_lock_for(t_timermutTimeout)==false)
            return false;
//    }
    //2-whether the queue is empty?
    if(m_Queue_MatDectOutput.size()==0)
        t_returnValue=false;
    else{
        t_returnValue=true;
        //3-clone
        o_matPicture=m_Queue_MatDectOutput.front();
        //4-pop
        m_Queue_MatDectOutput.pop();
    }
    //5-unlock
//    if(t_isMut == true)
        m_TimedMut_MatQueuePl.unlock();
    return t_returnValue;
}

bool TCV_DMHandle::ClearPictureInQueue()
{
    while(m_Queue_MatPl.size()!=0)
        m_Queue_MatPl.pop();

    while(m_Queue_MatVr.size()!=0)
        m_Queue_MatVr.pop();
    return true;
}

int TCV_DMHandle::GetQueuePlSize()
{
    return m_Queue_MatPl.size();
}

bool TCV_DMHandle::tcv_dectect(cv::Vec3f &o_distance, cv::Point &o_locate, int &o_errorNum)
{
    qDebug()<<"tcv_dectect";
//    if(m_DectHandle)
//        delete m_DectHandle;
    m_DectHandle = new TCV_DectHandle(this);
    m_DectHandle->start();
    connect(m_DectHandle, SIGNAL(SN_DectFinish(int)),
            this, SLOT(SL_DectFinishRecall(int)));

    return true;
}

bool TCV_DMHandle::tcv_measure(cv::Mat picture_left,
                               cv::Mat picture_right,
                               cv::Vec3f &o_distance,
                               cv::Point &o_locate,
                               int &o_errorNum)
{
    m_MeasureHandle = new TCV_MeasureHandle(picture_left, picture_right);
    if(m_MeasureHandle->MeasureDistance(100, o_distance)==-1){
        o_errorNum=-1;
        return false;
    }
    else {
        return true;
    }
}

void TCV_DMHandle::SL_DectFinishRecall(int signal_num)
{
    cv::Point t_point;
    if(signal_num == 1){
        m_DectHandle->GetDectResult(t_point);
        qDebug()<< "(" << t_point.x << "," << t_point.y << ")";
    }
    else if(signal_num == 0)
    {
        qDebug()<< "no target";
    }
    SN_DectFinishDMDect(t_point);


    qDebug()<< "thread recall" ;
}
